﻿#include <stdlib.h>
#include "stdafx.h"
#include "NCKHDemo.h"


using namespace cv;
using namespace System;
using namespace std;

// Khai bao cac hang can thiet
#define M  120
#define M1 60
#define W  50		/*W và H là kích thước cửa sổ sẽ resize */
#define H 50

#define TRAIN_SAMPLE_COUNT 120	/*Tổng số ảnh dùng để training*/
#define FEATURE_SIZE 60			/*Feature_Size bằng xấp sĩ 1/2 so với Train_Sample_Count*/

#define NUMBER_OF_CLASSES 6		/*Số Lớp nhận dạng*/

#define LoaiBien 6
#define SoLuongBien 20

//Khai bao bien toan cuc
const char * xmlfile="TrafficSignThaiTam.xml";
//Bien doc file
int K = W*H;
CvMat* eigenTT = cvCreateMat(K,M1,CV_32FC1);
CvMat* psi = cvCreateMat(K,1,CV_32FC1);

bool Co(IplImage *imgin)
{
	CvHaarClassifierCascade *cascade = (CvHaarClassifierCascade*)cvLoad(xmlfile);

	CvMemStorage* storage = cvCreateMemStorage();

	CvSeq *signal = cvHaarDetectObjects(
			imgin,
			cascade,
			storage,
			1.1,
			3,
			0,
			cvSize( 40, 40 ) );
	if (signal->total == 0)
		return false;
	else
		return true;
}

void DetectImage(IplImage *imgin,IplImage *imgout)
{
	//imgin se dc danh dau bien bao
	//imgout dung de xu ly nhan dang
	//imgout = cvCreateImage( cvSize(50,50), 8, 1 );
	//cvCopy(imgin, imgout);
	CvHaarClassifierCascade *cascade = (CvHaarClassifierCascade*)cvLoad(xmlfile);

	IplImage* gray = cvCreateImage( cvSize(50,50), 8, 1 );
	//cvCvtColor( imgout, gray, CV_BGR2GRAY );

	CvMemStorage* storage = cvCreateMemStorage();
	static CvScalar RED = {0, 0, 255};

	CvSeq *signal = cvHaarDetectObjects(
			imgin,
			cascade,
			storage,
			1.1,
			3,
			0,
			cvSize( 40, 40 ) );
	
	for (int i = 0; i < (signal ? signal->total : 0); i++)
    {
        CvRect *r = (CvRect*)cvGetSeqElem( signal, i );
        CvPoint ul; 
		CvPoint lr;
        ul.x = r->x;              ul.y = r->y; 
        lr.x = r->x + r->width;   lr.y = r->y + r->height;

		//Cat bien bao
		cvSetImageROI(imgin, cvRect(ul.x,ul.y,r->width,r->height));
		IplImage *cropped = cvCreateImage(cvGetSize(imgin),
                               imgin->depth,
                               imgin->nChannels);
		cvCopy(imgin, cropped, NULL);
		cvResetImageROI(imgin);
		
		//Resize kich thuoc bien bao ve 50 x 50
		IplImage *tempResize = cvCreateImage(cvSize(50,50),imgin->depth,imgin->nChannels);
		cvResize(cropped,tempResize);

		//Chuyen sang anh xam
		cvCvtColor( tempResize, gray, CV_BGR2GRAY );
		LoaiBoThongTinThua(gray,imgout);
		

		/*cvNamedWindow( "mywindow", CV_WINDOW_AUTOSIZE );
		cvShowImage( "mywindow", imgout );*/
		
		cvReleaseImage( &tempResize );
		//Ve danh dau bien bao banh mau do
		cvRectangle(imgin, ul, lr, RED, 3, 8, 0);
		
	}

	cvReleaseImage( &gray );
	TaoFilePhanLop();
}

IplImage* skipNFrames(CvCapture* capture, int n)
{
	for(int i = 0; i < n; ++i)
	{
		if(cvQueryFrame(capture) == NULL)
		{
			return NULL;
		}
	}
	return cvQueryFrame(capture);
}

void DetectVideo(char path[256])
{
	CvCapture *capture;
	IplImage  *frame;
	IplImage  *frame2 = cvCreateImage( cvSize(50,50), 8, 1 );
	int       key=0;

	char buffer[255];
	CvFont font;
	cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,0.5,0.5);

	capture = cvCaptureFromAVI(path);
	cvNamedWindow( "video", 1 );

	while( key != 'q' ) {
		frame = skipNFrames(capture,1);//cvQueryFrame( capture );

		if( !frame ) {
			return;
		}

		frame->origin = 0;
		
		bool ck = Co(frame);
		if(!ck) 
		{
			key = cvWaitKey( 10 );
			cvShowImage( "video", frame );	

		}
		else
		{
			DetectImage( frame,frame2 );
			//System::String^ name = gcnew System::String("D:\\APPLAUSE.WAV");
			//	//create an instance of the soundplayer class, sndPlayer can be a different name
			//	System::Media::SoundPlayer sndPlayer(name);
			//	sndPlayer.Play(); //there is also Stop(), and PlayLooping()
			key = cvWaitKey( 10 );
			float res = 0;
			res = DocFileEigenPsi(frame2);
			if (res == 1.0)
					sprintf(buffer,"Cho ngoat nguy hiem vong ben trai");
				else if (res == 2.0)
					sprintf(buffer,"Cho ngoat nguy hiem vong ben phai");
				else if (res == 3.0)
					sprintf(buffer,"Giao nhau voi tuyen duong cung cap");
				else if (res == 4.0)
					sprintf(buffer,"Giao nhau voi duong khong uu tien ben phai");
				else if (res == 5.0)
					sprintf(buffer,"Giao nhau voi duong sat co rao chan");
				else if (res == 6.0)
					sprintf(buffer,"Nguoi di bo");
				else
					sprintf(buffer,"Không xác định");
			
				cvPutText(frame,buffer,cvPoint(50,40),&font,CV_RGB(255,0,0));
				cvShowImage( "video", frame );
		}
					
	}
	cvWaitKey( 0 );
	//cvReleaseImage(&frame);
	cvReleaseCapture( &capture );
	cvDestroyWindow( "video" );
}

void LoaiBoThongTinThua(IplImage *imgin,IplImage *imgout)
{
	//Tao 3 diem toa do cuar 3 dinh hinh tam giac
	CvPoint p1, p2, p3;
	//IplImage *temp = cvCreateImage(cvGetSize(imgin),IPL_DEPTH_8U,1);
	IplImage *temp = cvCreateImage(cvGetSize(imgin),8,1);
	//imgout = cvCreateImage(cvSize(50,50),8,1);

	cvSetZero(imgout);

	p1 = cvPoint(0,cvGetSize(imgin).height-1);
	p2 = cvPoint(cvGetSize(imgin).width/2,0);
	p3 = cvPoint(cvGetSize(imgin).width-1,cvGetSize(imgin).height-1);

	CvPoint pts[3] = {p1,p2,p3};
	cvFillConvexPoly(temp,pts,3,cvScalar(255));

	int w = cvGetSize(imgin).width;
	int h = cvGetSize(imgin).height;
	int x,y;
	CvScalar value;
	for (x=0;x<w;x++)
		for (y=0;y<h;y++)
		{
			value = cvGet2D(temp,x,y);
			if (value.val[0] == 255)
				cvSet2D(imgout,x,y,cvGet2D(imgin,x,y));
		}
	
	cvReleaseImage(&temp);
}

void TaoCSDLHoc(char path[256])
{
	IplImage *tempin;
	IplImage *tempout = cvCreateImage(cvSize(50,50),8,1);
	int i,j;
	for(i=1; i<=LoaiBien; i++)
		for (j=1; j<=SoLuongBien; j++) {
			char filename[256];
			sprintf(filename,"\\TT%d\\TT (%d).bmp",i,j);
			char filepath[256];
			strcpy(filepath,path);
			strcat(filepath,filename);
			tempin = cvLoadImage(filepath,1);
			DetectImage(tempin,tempout);
			sprintf(filename,"TrafficSignTT\\TT%d\\TT(%d).bmp",i,j);
			cvSaveImage(filename,tempout);
		}
	cvReleaseImage(&tempin);
	cvReleaseImage(&tempout);

}

void TaoFilePhanLop()
{
	int i,j;
	FILE *fp;
	fp = fopen("PhanLop.txt","wt");
	for (i=1; i<=LoaiBien; i++)
		for (j=1; j<=SoLuongBien; j++) {
			fprintf(fp,"%d \n",i);
		}
	fclose(fp);
}

//--------------------------PCA_SVM---------------------------------//


void ReadFileClass(CvMat *trainClasses,char *filename)
{
	FILE *fp = fopen(filename,"rt");
	int i;
	float temp;
	CvScalar value;
	for (i=0; i<TRAIN_SAMPLE_COUNT; i++) {
		fscanf(fp,"%f",&temp);
		value.val[0] = temp;
		cvSet2D(trainClasses,i,0,value);
	}
	fclose(fp);
}

int SuDungSVMOpenCV(CvSVM* svm)
{
   svm->load("svmtrained.xml");
	return 0;
}

int KhoiTaoSVMOpenCV(CvSVM* svm, CvMat* trainData)
{
	
	CvMat* trainClasses = cvCreateMat(TRAIN_SAMPLE_COUNT, 1, CV_32FC1);

	ReadFileClass(trainClasses,"PhanLop.txt");

   CvSVMParams params = CvSVMParams(
												CvSVM::C_SVC,   // Type of SVM, here N classes (see manual)
												CvSVM::LINEAR,  // kernel type (see manual)
												0.0,			// kernel parameter (degree) for poly kernel only
												0.0,			// kernel parameter (gamma) for poly/rbf kernel only
												0.0,			// kernel parameter (coef0) for poly/sigmoid kernel only
												10,				// SVM optimization parameter C
												0,				// SVM optimization parameter nu (not used for N classe SVM)
												0,				// SVM optimization parameter p (not used for N classe SVM)
												NULL,			// class wieghts (or priors)
												cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 1000, 0.000001)
                             ); 
   fflush(NULL);

   // train using auto training parameter grid search if it is available
   // (i.e. OpenCV 2.x) with 10 fold cross valdiation
   // N.B. this does not search kernel choice

   svm->train_auto(trainData,trainClasses,
                        Mat(), Mat(), params, 10);

   svm->save("svmtrained.xml");

	cvReleaseMat(&trainClasses);
	return 0;
}

/*Ghi file eigenface.txt và psi.txt*/
void GhiFileEigenPsi()
{
	int k;

	CvMat* gamma = cvCreateMat(K,M,CV_32FC1);
	IplImage* temp;
	CvMat* psi = cvCreateMat(K,1,CV_32FC1);
	int x,y;
	
	// Tao lap bo nhan dang
	char filename[256];
	CvScalar value;
	int m=0;
	
	int i, j;
	
	//Chuyen anh thanh ma tran co kich thuoc W*HxM
	//tuc la ma tran voi moi cot la mot anh (moi anh la mot vecto cot)
	for (i=1; i<=LoaiBien; i++)
		for (j=1; j<=SoLuongBien; j++) {
			sprintf(filename,"TrafficSignTT\\TT%d\\TT(%d).bmp",i,j);
			temp = cvLoadImage(filename,CV_LOAD_IMAGE_GRAYSCALE);
			k=0;
			for (x=0; x<H; x++)
				for (y=0; y<W; y++) {
					value = cvGet2D(temp,x,y);
					cvSet2D(gamma,k,m,value);
					k++;
				}
			cvReleaseImage(&temp);
			m++;
		}

	//tinh gia tri trung binh
	for (k=0; k<K; k++) {
		value.val[0] = 0;
		for (m=0; m<M; m++) 
			value.val[0] += cvGet2D(gamma,k,m).val[0];
		value.val[0]/=M;
		cvSet2D(psi,k,0,value);
	}

	//lay anh trong gamma tru di anh trung binh psi
	for (k=0; k<K; k++) 
		for (m=0; m<M; m++) { 
			value.val[0] = cvGet2D(gamma,k,m).val[0]-cvGet2D(psi,k,0).val[0];
			cvSet2D(gamma,k,m,value);
		}

	CvMat *A  = cvCloneMat(gamma);
	CvMat *AT = cvCreateMat(M,K,CV_32FC1);
	cvTranspose(A,AT); //chuyen vi A tu K dong M cot sang M dong K cot
	CvMat *L  = cvCreateMat(M,M,CV_32FC1);
	cvMatMul(AT,A,L);

	CvMat *evects = cvCreateMat(M,M,CV_32FC1); //ma tran vector rieng
	CvMat *evals  = cvCreateMat(1,M,CV_32FC1); //ma tran gia tri rieng
	cvSetZero(evects);
	cvSetZero(evals);

	//tinh gia tri rieng va vector rieng
	cvEigenVV(L,evects,evals);

	CvMat* eigenTT = cvCreateMat(K,M1,CV_32FC1);
	int m1;
	for (m1=0; m1<M1; m1++)
		for (k=0; k<K; k++) {
			value.val[0] = 0;
			for (m=0; m<M; m++)
				value.val[0] += cvGet2D(gamma,k,m).val[0]*cvGet2D(evects,m1,m).val[0];
			cvSet2D(eigenTT,k,m1,value);		
		}

	
	CvMat *col   = cvCreateMat(K,1,CV_32FC1);
	CvMat *colT  = cvCreateMat(1,K,CV_32FC1);
	CvMat *w     = cvCreateMat(1,1,CV_32FC1);
	CvMat *omega = cvCreateMat(M,M1,CV_32FC1);
	for (m=0; m<M; m++)
		for (m1=0; m1<M1; m1++) {
			cvGetCol(eigenTT,col,m1);
			cvTranspose(col,colT);
			cvGetCol(gamma,col,m);
			cvMatMul(colT,col,w);
			cvSet2D(omega,m,m1,cvGet2D(w,0,0));
		}
	

   CvSVM* svm = new CvSVM; 
   KhoiTaoSVMOpenCV(svm,omega);
   	
	FILE *fp;
	double solieu;	
	fp = fopen("eigenTT.txt","wt");
	for (k=0; k<K; k++) {
		for (m1=0; m1<M1; m1++) {
			value= cvGet2D(eigenTT,k,m1);
			solieu=value.val[0];
			fprintf(fp,"%.15lf ",solieu);			
		}
		fprintf(fp,"\n");
	}
	fclose(fp);

	fp = fopen("psiTT.txt","wt");
	for (k=0; k<K; k++) {
		value= cvGet2D(psi,k,0);
		solieu=value.val[0];
		fprintf(fp,"%.15lf ",solieu);			
	}
	fprintf(fp,"\n");
	fclose(fp);
	return;
	
}

void DocFileEigen()
{
	int k;
	CvScalar value;
	double solieu;
	int m1;
	FILE *fp;
	fp = fopen("eigenTT.txt","rt");
	if(!fp)
		return;
	for (k=0; k<K; k++) {
		for (m1=0; m1<M1; m1++) {
			fscanf_s(fp,"%lf",&solieu);
			value.val[0]=solieu;
			value.val[1]=0;
			value.val[2]=0;
			value.val[3]=0;
			cvSet2D(eigenTT,k,m1,value);			
		}
	}
	fclose(fp);
}

void DocFilePsi()
{
	int k;
	CvScalar value;
	double solieu;
	FILE *fp;
	fp = fopen("psiTT.txt","rt");
	if(!fp)
		return;
	for (k=0; k<K; k++) {
		fscanf_s(fp,"%lf",&solieu);
		value.val[0]=solieu;
		value.val[1]=0;
		value.val[2]=0;
		value.val[3]=0;
		cvSet2D(psi,k,0,value);
	}
	fclose(fp);
}

/*Đọc file eigenface.txt và psi.txt và thực hiện nhận dạng*/
float DocFileEigenPsi(IplImage *imgin)
{
	
	int k, K = W*H;
	int x,y;
	
	// Tao lap bo nhan dang
	CvScalar value;

	CvSVM* svm = new CvSVM; 
	SuDungSVMOpenCV(svm);
	CvMat *col   = cvCreateMat(K,1,CV_32FC1);
	CvMat *colT  = cvCreateMat(1,K,CV_32FC1);
	CvMat *w     = cvCreateMat(1,1,CV_32FC1);
	

	/*DocFileEigen();
	DocFilePsi();*/
		int m1;
		
		//cvCopy(imgin,temp);
		CvMat* img = cvCreateMat(K,1,CV_32FC1);
		k=0;
		for (x=0; x<H; x++)
			for (y=0; y<W; y++) {
				value = cvGet2D(imgin,x,y);
				cvSet2D(img,k,0,value);
				k++;
			}

			for (k=0; k<K; k++) {
				value.val[0] = cvGet2D(img,k,0).val[0]-cvGet2D(psi,k,0).val[0];
				cvSet2D(img,k,0,value);
			}


			CvMat *data = cvCreateMat(1,M1,CV_32FC1);
			value = cvGet2D(eigenTT,K-1,M1-1);
			for (m1=0; m1<M1; m1++) {
				cvGetCol(eigenTT,col,m1);
				cvTranspose(col,colT);
				cvGetCol(img,col,0);
				cvMatMul(colT,col,w);
				cvSet2D(data,0,m1,cvGet2D(w,0,0));

			}

			for (m1 = 0; m1 <M1; m1++)
				value = cvGet2D(data,0,m1);

			float response;
			response = svm->predict(data);
			
	return response;
}


